// #include <ros/ros.h>
// #include <sensor_msgs/Encoder.h>  // 假设使用轮编码器数据
// #include <nav_msgs/Odometry.h>
// #include <tf/transform_broadcaster.h>
// #include <tf/transform_datatypes.h>

// class OdometryNode {
// public:
//     OdometryNode() : x_(0.0), y_(0.0), theta_(0.0) {
//         // 初始化ROS节点
//         ros::NodeHandle nh;

//         // 获取参数
//         nh.param("wheel_radius", wheel_radius_, 0.1);
//         nh.param("wheel_base", wheel_base_, 0.5);
//         nh.param("ticks_per_meter", ticks_per_meter_, 1000.0);

//         // 订阅轮编码器数据
//         encoder_sub_ = nh.subscribe("/encoder", 10, &OdometryNode::encoderCallback, this);

//         // 发布里程计数据
//         odom_pub_ = nh.advertise<nav_msgs::Odometry>("/odom", 50);

//         // TF广播器
//         tf_broadcaster_ = std::make_shared<tf::TransformBroadcaster>();

//         // 初始化时间
//         last_time_ = ros::Time::now();
//     }

// private:
//     double wheel_radius_;
//     double wheel_base_;
//     double ticks_per_meter_;
//     double x_, y_, theta_;
//     ros::Time last_time_;
//     ros::Subscriber encoder_sub_;
//     ros::Publisher odom_pub_;
//     std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;

//     void encoderCallback(const sensor_msgs::Encoder::ConstPtr& msg) {
//         // 获取当前时间
//         ros::Time current_time = ros::Time::now();
//         double dt = (current_time - last_time_).toSec();

//         // 解析轮编码器数据
//         double left_ticks = msg->left_ticks;
//         double right_ticks = msg->right_ticks;

//         // 计算线速度和角速度
//         double left_distance = (left_ticks / ticks_per_meter_) * wheel_radius_;
//         double right_distance = (right_ticks / ticks_per_meter_) * wheel_radius_;
//         double linear_x = (left_distance + right_distance) / 2.0;
//         double angular_z = (right_distance - left_distance) / wheel_base_;

//         // 计算新的位置和姿态
//         x_ += linear_x * std::cos(theta_);
//         y_ += linear_x * std::sin(theta_);
//         theta_ += angular_z;

//         // 发布里程计数据
//         nav_msgs::Odometry odom;
//         odom.header.stamp = current_time;
//         odom.header.frame_id = "odom";
//         odom.child_frame_id = "base_footprint";

//         odom.pose.pose.position.x = x_;
//         odom.pose.pose.position.y = y_;
//         odom.pose.pose.position.z = 0.0;
//         odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_);

//         odom.twist.twist.linear.x = linear_x / dt;
//         odom.twist.twist.angular.z = angular_z / dt;

//         odom_pub_.publish(odom);

//         // 发布TF变换
//         geometry_msgs::TransformStamped odom_tf;
//         odom_tf.header.stamp = current_time;
//         odom_tf.header.frame_id = "odom";
//         odom_tf.child_frame_id = "base_footprint";

//         odom_tf.transform.translation.x = x_;
//         odom_tf.transform.translation.y = y_;
//         odom_tf.transform.translation.z = 0.0;
//         odom_tf.transform.rotation = odom.pose.pose.orientation;

//         tf_broadcaster_->sendTransform(odom_tf);

//         // 更新时间
//         last_time_ = current_time;
//     }
// };

// int main(int argc, char** argv) {
//     ros::init(argc, argv, "odometry_node");
//     OdometryNode odometry_node;
//     ros::spin();
//     return 0;
// }
